/**********************************************************************************************************************
 * @file    icm42670.c
 * @author  Queclink Billy.Luo
 * @date    2021-5-31
 * @brief   icm42670驱动程序
 *
 * Copyright (C) 2021 Queclink Wireless Solutions (ShenZhen) Co., Ltd. All Rights Reserved.
 *
 * @attention
 *********************************************************************************************************************/

#include "icm42670.h"

int32_t icm42670_reg_set(context_t *ctx, uint8_t reg, uint8_t val)
{
    return ctx->write_reg(reg, &val, 1);
}

int32_t icm42670_reg_get(context_t *ctx, uint8_t reg, uint8_t *val)
{
    return ctx->read_reg(reg, val, 1);
}

float icm42670_temperature_get(context_t *ctx)
{
    int ret;
    uint8_t data[2];
    int16_t offset;
    
    ret = ctx->read_reg(TEMP_DATAH_REG, &data[1], 1);
    if(0 != ret)
        return -1;

	ret = ctx->read_reg(TEMP_DATAL_REG, &data[0], 1);
    if(0 != ret)
        return -1;
        
    offset = data[1] << 8 | data[0];
    return 25.0 + ((float)offset / 128);
}

int32_t icm42670_accel_raw_get(context_t *ctx, uint8_t *buff)
{
    return ctx->read_reg(ACCEL_DATA_XH_REG, buff, 6);
}

int32_t icm42670_gyro_raw_get(context_t *ctx, uint8_t *buff)
{
    return ctx->read_reg(GYRO_DATA_XH_REG, buff, 6);
}

int32_t icm42670_fifo_wm_level_set(context_t *ctx, uint16_t level)
{
    int ret;
    uint8_t val;
    
    val = (level >> 8) & 0x0F;
    ret = ctx->write_reg(FIFO_CONFIG3_REG, (uint8_t *)&val, 1);
    val = level & 0x00FF;
    ret += ctx->write_reg(FIFO_CONFIG2_REG, (uint8_t *)&val, 1);
    return ret;
}

int32_t icm42670_fifo_data_cnt_get(context_t *ctx, uint16_t *cnt)
{
    int ret;
    icm_reg_t reg;
    
    ret = ctx->read_reg(FIFO_COUNTL_REG, &reg.byte, 1);
    *cnt = reg.byte;
    ret = ctx->read_reg(FIFO_COUNTH_REG, &reg.byte, 1);
    *cnt |= reg.byte << 8;
    
    return ret;
}

int32_t icm42670_fifo_packet_data_read(context_t *ctx, uint8_t buff[16])
{
    return ctx->read_reg(FIFO_DATA_REG, buff, 16);
}

int32_t bank_reg_set(context_t *ctx, uint8_t bank, uint8_t reg, uint8_t data)
{
    int ret;
    
    switch(bank) {
    case ICM_BANK1:
        bank = 0x00;
        break;
    case ICM_BANK2:
        bank = 0x28;
        break;
    case ICM_BANK3:
        bank = 0x50;
        break;
    default:
        bank = 0x00;
        break;
    }
    
    ret = ctx->write_reg(BANK_SEL_W, &bank, 1);
    ret += ctx->write_reg(MADDR_W, &reg, 1);
    ret += ctx->write_reg(MDATA_W, &data, 1);
    ctx->delay_ms(1);
    
    if(0x00 != bank) {
        bank = 0x00;
        ret += ctx->write_reg(BANK_SEL_W, &bank, 1);
    }
    
    return ret;
}
